/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2023 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <math.h>
#include <string.h>
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
#define MODEL
// #define TEST

#define LED_OFF HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_SET)
#define LED_ON HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_RESET)
#define MAX_N 100  // store angle displacement and speed array length
#define PI 3.14
#define MOTOR_PULSE 4096
#define TARGET(x) (sin(x) + 2)

#ifdef TEST
/***** 测试 *****/
char *send_info = "";
/***** 测试 *****/
// useless
typedef union {
    float fdata;
    unsigned long ldata;
} FloatType;

typedef union {
    int idata;
    unsigned long ldata;
} IntType;

void Float_to_Byte(float f, unsigned char byte[]);
void Int_to_Byte(int n, unsigned char byte[]);
#endif

unsigned short GET_RS485_MODBUS_RTU_CRC(unsigned char *data, unsigned short length);
void ElectricMotorInit(void);
float g1(float x);
float g2(float x);

uint8_t receive_info[16];
float send_data[3];

#ifndef MODEL
// Don't consider the situation of (front - back > MAX_N) temporarily
float angle_displacement[MAX_N] = {0};
float angle_speed[MAX_N] = {0};
static uint32_t front, back;
#else
uint32_t K1 = 5202;
float K2 = -6.375;
float factor = 0.0001;  // 调节因子
float angle_displacement;
float angle_speed;
#endif
/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_USART1_UART_Init();
  MX_TIM4_Init();
  MX_TIM3_Init();
  MX_USART2_UART_Init();
  /* USER CODE BEGIN 2 */
    // �?启编码器4，定时器3
#ifndef MODEL
    HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_1 | TIM_CHANNEL_2);
#endif
    HAL_TIM_Base_Start_IT(&htim3);
    // 电机初始�?
    ElectricMotorInit();
    // 设置参数:
    uint8_t beta1 = 50, beta2 = 200, beta3 = 200, a = 20;
    uint32_t b0 = 5202;
    // initialization
    float z1 = 0, z2 = 0, z3 = 0, u = 0, goal = 0;
    float e1 = 0, e2 = 0;  // e1 is the error of angle displacement, and e2 is the other
    // uint8_t ch = 0;
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
    while (1) {
        /* LED indicator light
        LED_OFF;
        HAL_Delay(500);
        LED_ON;
        HAL_Delay(500);
        */
        /* uart register transmitting and sending test
        ch = UartGetchar();
        UartPutchar(ch);
        */
        
#ifndef MODEL
        if (back >= front)
            continue;
        else
            ++back;
        uint8_t index = back % MAX_N;
        /* handle receive info from encoder */
        e1 = z1 - angle_displacement[index];
        e2 = z2 - angle_speed[index];
#else
        e1 = z1 - angle_displacement;
        e2 = z2 - angle_speed;
#endif
        z1 = z1 + (z2 - beta1 * e1) * factor;
        z2 = z2 + (z3 - beta2 * g1(e2) + b0 * u) * factor;
        z3 = z3 + (-beta3 * g2(e2)) * factor;
        /***** 测试 *****/
        if (goal > 2 * PI)
            goal = 0;
        else
            goal += (2 * PI) / 20000;
        /***** 测试 *****/
#ifndef MODEL
        u = (-z3 + pow(a, 2) * (TARGET(test) - angle_displacement[index]) - 2 * a * angle_speed[index]) / b0;
#else
        u = (-z3 + pow(a, 2) * (TARGET(goal) - angle_displacement) - 2 * a * angle_speed) / b0;
#endif
        // 电机位置
        int32_t motor_position = (int32_t)(u * 436 / 5 * MOTOR_PULSE);
        uint8_t total_data[15] = {0};
        uint8_t pre_data[] = {0x01, 0x10, 0x10, 0x08, 0x00, 0x02, 0x04};
        uint8_t data_length = sizeof(pre_data);
        memcpy(total_data, pre_data, data_length);
        memcpy(total_data + data_length, (uint8_t *)&motor_position, 4);
        data_length = data_length + 4;
		// CRC 校验�?
        unsigned short res = GET_RS485_MODBUS_RTU_CRC(total_data, data_length);
        memcpy(total_data + data_length, (uint8_t *)&res, 2);
        // send information to motor
        HAL_UART_Transmit(&huart1, total_data, sizeof(total_data), 100);  // exceed 100ms, stop!
#ifdef TEST
        // 测试中断与主函数的运行频�?
        send_info = "123";
        HAL_UART_Transmit(&huart1, (uint8_t*)send_info, sizeof(send_info), 100);
#endif
		// vofa+ graph
        send_data[0] = TARGET(goal);
        send_data[1] = angle_displacement;
        send_data[2] = angle_speed;
        static uint8_t to_vofa[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x80, 0x7f};
        memcpy(to_vofa, (uint8_t *)send_data, sizeof(send_data));
        HAL_UART_Transmit(&huart2, to_vofa, sizeof(to_vofa), 100);
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
    }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI_DIV2;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */
float g1(float x) 
{
    return (pow(fabs(x), 0.5) * ((x > 0) ? 1 : ((x == 0) ? 0 : -1)));
}

float g2(float x) 
{
    return (pow(fabs(x), 0.25) * ((x > 0) ? 1 : ((x == 0) ? 0 : -1)));
}

// 定时�?3 中断函数
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim) 
{
    if (htim->Instance == TIM3) {
        /* 计算角位移，角�?�度 */
        // 测量间隔 1ms
#ifndef MODEL
        int cnt = __HAL_TIM_GET_COUNTER(&htim4) - 0x7fff;
        __HAL_TIM_SET_COUNTER(&htim4, 0x7fff);
        float angle_dis = ((float)cnt / 4 / 1000) * 2 * PI;
        float angle_spd = angle_dis / 1;  // 1ms
        // 加入各自数组
        if (front == UINT32_MAX) {
            front = 0;
            back = 0;
        }
        uint8_t index = front % MAX_N;
        if (index == 0) {
                angle_displacement[index] = angle_displacement[MAX_N - 1] + angle_dis;
        } else {
                angle_displacement[index] = angle_displacement[index - 1] + angle_dis;
        }
        angle_speed[index] = angle_spd;
        ++front;
#else
        uint8_t read_motor_position[] = {0x01, 0x03, 0x13, 0x08, 0x00, 0x02, 0x41, 0x4D};
        HAL_UART_Transmit(&huart1, read_motor_position, sizeof(read_motor_position), 100);  // exceed 100ms, stop!
        HAL_UART_Receive(&huart1, receive_info, sizeof(receive_info), 100);
        // handle receive data
        int d_size = sizeof(receive_info);
        unsigned short test = GET_RS485_MODBUS_RTU_CRC(receive_info, d_size - 2);
        if (test == (receive_info[d_size - 1] << 8 | receive_info[d_size - 2])) {
            int motor_position = receive_info[3] << 24 | receive_info[4] << 16 | receive_info[5] << 8 | receive_info[6];
            float u = motor_position / MOTOR_PULSE / 436 * 5;
            angle_displacement = angle_displacement + angle_speed * factor;
            angle_speed = angle_speed + (K2 * angle_speed + K1 * u) * factor;
        }
        memset(receive_info, 0, sizeof(receive_info));
#endif
#ifdef TEST
        // 测试中断与主函数的运行频�?
        send_info = "456";
        HAL_UART_Transmit(&huart1, (uint8_t *)send_info, sizeof(send_info), 100);
        // 测试阶段
        if (index == 0) {
            float last_speed = angle_speed[MAX_N - 1];
            angle_displacement[index] = angle_displacement[MAX_N - 1] + last_speed * factor;
            angle_speed[index] = last_speed + (K2 * last_speed + K1 * u) * factor;
        } else {
            float last_speed = angle_speed[index - 1];
            angle_displacement[index] = angle_displacement[index - 1] + last_speed * factor;
            angle_speed[index] = last_speed + (K2 * last_speed + K1 * u) * factor;
        }
        // vofa+ 画图
        static uint8_t to_vofa[4] = {0};
        memcpy(to_vofa, (uint8_t *)&cnt, sizeof(cnt));
        HAL_UART_Transmit(&huart1, to_vofa, sizeof(to_vofa), 100);
        uint8_t tail[] = {0x00, 0x00, 0x80, 0x7f};
        HAL_UART_Transmit(&huart2, tail, sizeof(tail), 100);
#endif
    }
}

// 电机初始化：设置工作模式
//           清空位置反馈、给定信�?
//           给电机使�?
void ElectricMotorInit() 
{
    // 电机工作模式：位置环
    uint8_t posit_mode[] = {0x01, 0x06, 0x10, 0x0A, 0x00, 0x00, 0xAD, 0x08};
    HAL_UART_Transmit(&huart1, posit_mode, sizeof(posit_mode), 100);  // exceed 100ms, stop!
    // from electric motor data
    HAL_UART_Receive(&huart1, receive_info, sizeof(receive_info), 100);
    while (memcmp(posit_mode, receive_info, sizeof(posit_mode))) {
        LED_OFF;
        memset(receive_info, 0, sizeof(receive_info));
        HAL_UART_Transmit(&huart1, posit_mode, sizeof(posit_mode), 100);
        HAL_UART_Receive(&huart1, receive_info, sizeof(receive_info), 100);
    }
    memset(receive_info, 0, sizeof(receive_info));
    LED_ON;
    // 清空电机位置反馈值�?�位置给定�??
    uint8_t clear_data[] = {0x01, 0x06, 0x10, 0x24, 0x00, 0x02, 0x4C, 0xC0};
    HAL_UART_Transmit(&huart1, clear_data, sizeof(clear_data), 100);  // exceed 100ms, stop!
    // 给电机使�?
    uint8_t enable[] = {0x01, 0x06, 0x10, 0x00, 0x00, 0x0A, 0x0D, 0x0D};
    HAL_UART_Transmit(&huart1, enable, sizeof(enable), 100);  // exceed 100ms, stop!
#ifdef TEST
    // from electric motor data
    HAL_UART_Receive(&huart1, receive_info, sizeof(receive_info), 100);
    while (memcmp(enable, receive_info, sizeof(enable))) {
    	LED_OFF;
        memset(receive_info, 0, sizeof(receive_info));
    	HAL_UART_Transmit(&huart1, enable, sizeof(enable), 100);
        HAL_UART_Receive(&huart1, receive_info, sizeof(receive_info), 100);
    }
    memset(receive_info, 0, sizeof(receive_info));
    LED_ON;
#endif
}

// CRC 校验
unsigned short GET_RS485_MODBUS_RTU_CRC(unsigned char *data, unsigned short length) 
{
    unsigned short j;
    unsigned short reg_crc = 0xffff;
    while (length--) {
        reg_crc ^= *data++;
        for (j = 0; j < 8; j++) {
            if (reg_crc & 0x01) {
                reg_crc = (reg_crc >> 1) ^ 0xa001;
            } else {
                reg_crc = reg_crc >> 1;
            }
        }
    }
    return reg_crc;
}

/* useless
// �? 整数n 转化�? 4个字节数据并按照大端重新排列存放�? byte[4]
void Int_to_Byte(int n, unsigned char byte[])
{
    IntType integer;
    integer.idata = n;
    byte[1] = (unsigned char)integer.ldata;
    byte[0] = (unsigned char)(integer.ldata >> 8);
    byte[3] = (unsigned char)(integer.ldata >> 16);
    byte[2] = (unsigned char)(integer.ldata >> 24);
}

// �? 浮点数f 转化�? 4个字节数�? 存放�? byte[4]
void Float_to_Byte(float f, unsigned char byte[]) 
{
    FloatType fl;
    fl.fdata = f;
    byte[0] = (unsigned char)fl.ldata;
    byte[1] = (unsigned char)(fl.ldata >> 8);
    byte[2] = (unsigned char)(fl.ldata >> 16);
    byte[3] = (unsigned char)(fl.ldata >> 24);
} */
/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
    /* User can add his own implementation to report the HAL error return state */
    __disable_irq();
    while (1) {
    }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
    /* User can add his own implementation to report the file name and line number,
       ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
